/*
frigOS
Licensed under GPLv3
*/

#ifndef DISABLE_AX12

#include "ax12.h"
#include "kernel.h"
#include "protocol.h"
#include "serial.h"
#include "timer.h"
#include "led.h"
#include "shared_memory.h"
#include "avr/interrupt.h"
#include "dynamixel.h"

// for sync set
// n = 20(NUMBER_OF_SERVOS) * 5 + 2 = 102
// buffer_size = n + 6 = 108
// length = n + 2 = 104
#define SIZE_OF_N       (NUMBER_OF_SERVOS * 5 + 2)  // n = 20(NUMBER_OF_SERVOS) * 5 + 2 = 102
#define BUFFER_LENGTH   (SIZE_OF_N + 6)             // buffer_size = n + 6 = 108
#define POSITION_LENGTH (SIZE_OF_N + 2)             // length = n + 2 = 104

// initialize a servo
void initServo(uint8_t id)
{
    //clear error staus
    writeByte(id, AX12_ALARM_LED, 0, NULL);
    writeByte(id, AX12_ALARM_SHUTDOWN, 0, NULL);
    //enable re-enable torque, to clear error condition.
    writeByte(id, AX12_TORQUE_ENABLE, 1, NULL);
    //writeByte(id, AX12_TORQUE_ENABLE, 0, NULL, 0);
}

// get the high and low bytes of the servo's current position and return them
// returns 0x0000 if there was an error
uint16_t getPosition(uint8_t id)
{
    DXL_Packet response;
    uint16_t result = 0x0000;

    readBytes(id, AX12_PRESENT_POSITION_L, 0x02, &response);


    if(response.instrErr == 0)
    {
        result = (uint16_t)response.params[0] | (((uint16_t)(response.params[1]))<<8);
    }

    return result;
}

// get the high and low bytes of the servo's current load and return them
uint16_t getLoad(uint8_t id)
{
    DXL_Packet response;
    uint16_t result = 0x0000;

    readBytes(id, AX12_PRESENT_LOAD_L, 0x02, &response);


    if(response.instrErr == 0)
    {
        result = (uint16_t)response.params[0] | (((uint16_t)(response.params[1]))<<8);
    }

    return result;
}

// get the high and low bytes of the servo's current speed and return them
uint16_t getSpeed(uint8_t id)
{
    DXL_Packet response;
    uint16_t result = 0x0000;

    readBytes(id, AX12_PRESENT_SPEED_L, 0x02, &response);


    if(response.instrErr == 0)
    {
        result = (uint16_t)response.params[0] | (((uint16_t)(response.params[1]))<<8);
    }

    return result;
}

// set the servo's current position
void setGoalPosition(uint8_t id, uint16_t position)
{

}

// set the servo's current speed
void setSpeed(uint8_t id, uint16_t speed)
{

}


void syncSetPositionAndSpeed(uint8_t const positions[], uint16_t speed)
{

    uint8_t buffer[BUFFER_LENGTH];
    uint8_t x;

    buffer[0] = DXL_HEADER1;
    buffer[1] = DXL_HEADER2;
    buffer[2] = DXL_BROADCAST_ID;
    buffer[3] = POSITION_LENGTH; // length
    buffer[4] = DXL_SYNC_WRITE;
    buffer[5] = AX12_GOAL_POSITION_L;
    buffer[6] = 0x04;

    uint8_t i = 7;

    for (x = 0; x < NUMBER_OF_SERVOS; x++)
    {

        uint16_t position = positions[x] * 4;
        buffer[i++] = x + 1; // id
        buffer[i++] = (uint8_t) position; // position_l
        buffer[i++] = (uint8_t) (position >> 8); // position_h
        buffer[i++] = (uint8_t) speed; // speed_l
        buffer[i++] = (uint8_t) (speed >> 8); // speed_h

    }

    buffer[i] = calculateCheckSum( DXL_BROADCAST_ID, DXL_SYNC_WRITE, &buffer[5], SIZE_OF_N );

    lockSemaphore(&usart0_lock);
    transmitBufferUSART0( buffer, BUFFER_LENGTH );
    unlockSemaphore(&usart0_lock);

}

#ifdef HW_DEBUG
void testMotors()
{
    // alternate main used only for hardware/servo debugging
    // uses no threads & no interrupts

    cli();

    initRS485();
    initUSART0();
    initUSART1(USART1_DEFAULT_BAUD, FALSE);
    initLEDs();

    for(;;)
    {
        // get FF FF ID ~ID from USART1
        // then read the position & torque from servo ID
        // and send the results over USART1 in ASCII

        transmitStringUSART1_busyLoop("\nSend FF FF ID ~ID to get data for servo #ID\n");

        uint8_t inLength = 0;   // the length of the incoming packet so far
        uint8_t buffIn[24];     // place to store data pulled in from serial
        uint8_t buffOut[24];    // place to store data to transmit
        uint8_t servoID;
        uint8_t checksum;

        const uint8_t REPLY_LENGTH = 6; //12;
        const uint8_t TRANSMIT_LENGTH = 6;  //8

        while(inLength!=4)
        {
            // wait for a byte to come in over USART1
            while( !(UCSR1A & (1 << RXC1)) )
                asm volatile("nop"::);

            buffIn[inLength] = UDR1;
            inLength++;
        }


        checksum = buffIn[3];
        checksum = ~checksum;

        // if the packet is good then we can keep going
        if(buffIn[0] == 0xff && buffIn[1] == 0xff && buffIn[2] == checksum)
        {
            servoID = buffIn[2];

            byte2hex(servoID, buffOut);
            transmitStringUSART1_busyLoop("Checking servo #");
            transmitBufferUSART1_busyLoop(buffOut, 2);
            transmitStringUSART1_busyLoop("...\n");

            buffOut[0] = 0xff;
            buffOut[1] = 0xff;
            buffOut[2] = servoID;

            // READ DATA
            //buffOut[3] = 4; //#params +2
            //buffOut[4] = AX12_READ_DATA;
            //buffOut[5] = AX12_PRESENT_POSITION_L;  // start reading from this address
            //buffOut[6] = 6;                             // read 6 bytes
            //buffOut[7] = calculateCheckSum(servoID, AX12_READ_DATA, &(buffOut[USART0_PACKET_PARAMETER_1_BYTE]), 2);

            // WRITE DATA
            //buffOut[3] = 4;
            //buffOut[4] = AX12_WRITE_DATA;
            //buffOut[5] = AX12_LED;
            //buffOut[6] = 1;
            //buffOut[7] = calculateCheckSum(servoID, AX12_WRITE_DATA, &(buffOut[USART0_PACKET_PARAMETER_1_BYTE]), 0);

            // PING
            buffOut[3] = 2;
            buffOut[4] = AX12_PING;
            buffOut[5] = calculateCheckSum(servoID, AX12_PING, NULL, 0);

            transmitStringUSART1_busyLoop("Transmitting: ");
            transmitASCIIBufferUSART1_busyLoop(buffOut, TRANSMIT_LENGTH);
            transmitStringUSART1_busyLoop("\n");

            setTxUSART0();
            transmitBufferUSART0_busyLoop(buffOut, TRANSMIT_LENGTH);
            setRxUSART0();

            manageLEDOn();

            // read in the packet from the servo
            uint16_t cycles = 0;
            uint16_t waitTime;
            inLength = 0;
            while(inLength<REPLY_LENGTH && cycles < 500)
            {
                waitTime = 0;
                // wait for a byte to come in over USART0
                while( !(UCSR0A & (1 << RXC0)) && waitTime < 50000 )
                {
                    //asm volatile("nop"::);
                    waitTime++;
                }

                if(waitTime < 50000)
                {
                    buffIn[inLength] = UDR0;
                    inLength++;
                }
                cycles++;
            }

            manageLEDOff();

            if(inLength==REPLY_LENGTH)
            {
                // check the packet
                if(buffIn[0] == 0xff && buffIn[1] == 0xff)
                {
                    uint16_t sum = 0;
                    uint8_t i;
                    for(i=2; i<REPLY_LENGTH-1; i++)
                        sum += buffIn[i];
                    sum = ~sum;

                    if(((uint8_t) sum) == buffIn[REPLY_LENGTH-1])
                    {
                        /*
                        transmitStringUSART1_busyLoop("Servo status:\n");

                        transmitStringUSART1_busyLoop("\tPosition: ");
                        byte2hex(buffIn[5], buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        byte2hex(buffIn[6], buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        transmitStringUSART1_busyLoop("\n");

                        transmitStringUSART1_busyLoop("\tSpeed: ");
                        byte2hex(buffIn[7], buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        byte2hex(buffIn[8], buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        transmitStringUSART1_busyLoop("\n");

                        transmitStringUSART1_busyLoop("\tLoad: ");
                        byte2hex(buffIn[9], buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        byte2hex(buffIn[10], buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        transmitStringUSART1_busyLoop("\n");
                        */

                        transmitStringUSART1_busyLoop("Successfully pinged servo #");
                        byte2hex(servoID, buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        transmitStringUSART1_busyLoop("\n");

                        transmitStringUSART1_busyLoop("\tError Code: ");
                        byte2hex(buffIn[USART0_PACKET_ERROR_BYTE],buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        transmitStringUSART1_busyLoop("\n");
                    }
                    else
                    {
                        transmitStringUSART1_busyLoop("Invalid checksum recieved from servo\n");
                        transmitStringUSART1_busyLoop("Received: ");
                        byte2hex(buffIn[REPLY_LENGTH-1], buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        transmitStringUSART1_busyLoop("\nExpected: ");
                        byte2hex((uint8_t)sum, buffOut);
                        transmitBufferUSART1_busyLoop(buffOut, 2);
                        transmitStringUSART1_busyLoop("\n");
                    }
                }
                else
                {
                    transmitStringUSART1_busyLoop("Invalid packet header recieved from servo\n");
                    transmitStringUSART1_busyLoop("Received: ");
                    byte2hex(buffIn[0], buffOut);
                    transmitBufferUSART1_busyLoop(buffOut, 2);
                    byte2hex(buffIn[1], buffOut);
                    transmitBufferUSART1_busyLoop(buffOut, 2);
                    transmitStringUSART1_busyLoop("\n");
                }
            }
            else
            {
                transmitStringUSART1_busyLoop("Timed out waiting for reply\n");
                transmitStringUSART1_busyLoop("Received ");
                byte2hex(inLength, buffOut);
                transmitBufferUSART1_busyLoop(buffOut, 2);
                transmitStringUSART1_busyLoop(" bytes\n");
                transmitStringUSART1_busyLoop("Raw data: ");
                transmitASCIIBufferUSART1_busyLoop(buffIn, inLength);
                transmitStringUSART1_busyLoop("\n");
            }
        }
        else
        {
            transmitStringUSART1_busyLoop("Invalid packet recieved from PC\n\tID: ");
            byte2hex(buffIn[2], buffOut);
            transmitBufferUSART1_busyLoop(buffOut, 2);
            transmitStringUSART1_busyLoop("\n\tChecksum: ");
            byte2hex(buffIn[3], buffOut);
            transmitBufferUSART1_busyLoop(buffOut, 2);
            transmitStringUSART1_busyLoop("\n");
        }
    }
}
#endif
/*
void handleCommand(void *arg)
{

    lockSemaphore(&shared_usart1_rx_buffer_lock);

    // copy usart1 rx buffer
    uint8_t buffer_length = *( (uint8_t *) arg );// - 1;
    uint8_t buffer[USART1_RX_BUFFER_SIZE];

    uint8_t x;
    for (x = 0; x < buffer_length; x++)
        buffer[x] = shared_usart1_rx_buffer[x];

    uint8_t checksum = shared_usart1_rx_buffer[x];

    unlockSemaphore(&shared_usart1_rx_buffer_lock);

    if ( checksum == protocolCheckSum(buffer, buffer_length) )
    {

        switch(buffer[0])
        {

        case CMD_GET_POSITION:
            getPosition(buffer[1], REPLY_ON);
            break;

        case CMD_SET_POSITION_AND_SPEED:
            setPositionAndSpeed(buffer[1], buffer[2], buffer[3], REPLY_ON);
            break;

        case CMD_MOTOR_OFF:
            motorOff(buffer[1], REPLY_ON);
            break;

        case CMD_MOTOR_ON:
            motorOn(buffer[1], REPLY_ON);
            break;

        case CMD_PING:
            ping(buffer[1], REPLY_ON);
            break;

        case CMD_MOVE_LOAD_CHECK:
            moveLoadCheck(buffer[1], buffer[2], REPLY_ON);
            break;
        default:
            break;

        }

    }
    else
    {

        protocolFail();

    }

} // handleCommand

// id length error parameter1 parameter2 ... parameterN checksum
void handleReply(void *arg)
{

    lockSemaphore(&shared_usart0_rx_buffer_lock);

    // copy usart0 rx buffer
    uint8_t buffer_length = *( (uint8_t *) arg ) - 1;
    uint8_t buffer[USART0_RX_BUFFER_SIZE];

    uint8_t x;
    for (x = 0; x < buffer_length; x++)
        buffer[x] = shared_usart0_rx_buffer[x];

    uint8_t checksum = shared_usart0_rx_buffer[x];

    unlockSemaphore(&shared_usart0_rx_buffer_lock);

    uint8_t n = 0;
    uint8_t *parameters = 0;

    if (buffer[1] > 2)
    {

        n = buffer[1] - 2;
        parameters = &buffer[3];

    }

    if ( checksum == calculateCheckSum(buffer[0], 0, parameters, n) )
    {

        uint8_t reply_length = buffer[1] + 3;
        uint8_t reply[16];
        reply[0] = 0xff;
        reply[1] = buffer[0]; // id
        reply[2] = buffer[1]; // length
        reply[3] = buffer[2]; // error

        for (x = 0; x < n; x++)
            reply[x + 4] = buffer[x + 3]; // parameters

        reply[reply_length - 1] = protocolCheckSum(&reply[1], reply_length - 2); // checksum
        lockSemaphore(&usart1_tx_lock);
        transmitBufferUSART1(reply, reply_length);
        unlockSemaphore(&usart1_tx_lock);

    }
    else
    {

        protocolFail();

    }

} // handleReply

// Check Sum = ~ (ID + Length + Instruction + Parameter1 + ... Parameter N)
// If the calculated value is larger than 255, the lower byte is defined as the checksum
// length = n + 2
uint8_t calculateCheckSum(uint8_t id, uint8_t instruction, uint8_t const parameters[], uint8_t n)
{

    uint16_t result = id + (n + 2) + instruction;
    uint8_t x;
    for (x = 0; x < n; x++)
        result += parameters[x];

    return (uint8_t) (~result);

} // calculateCheckSum

uint8_t getPosition(uint8_t id, uint8_t replyStatus)
{

    setReplyStatus(replyStatus);

    uint8_t result = 0;

    uint8_t buffer[8];
    uint8_t x;

    // 0xff 0xff id length instruction address length checksum
    buffer[0] = AX12_HEADER0;
    buffer[1] = AX12_HEADER1;
    buffer[2] = id;
    buffer[3] = 0x04;
    buffer[4] = AX12_READ_DATA;
    buffer[5] = AX12_PRESENT_POSITION_L;
    buffer[6] = 0x02;
    buffer[7] = calculateCheckSum(id, AX12_READ_DATA, &buffer[5], 0x02);

    lockSemaphore(&usart0_lock);
    transmitBufferUSART0(buffer, 8);
    unlockSemaphore(&usart0_lock);

    uint16_t start_time = ticks1;
    while( shared_usart0_rx_buffer_length < 6 || (ticks1 - start_time) < AX12_REPLY_TIMEOUT )
        asm volatile("nop"::);

    uint8_t reply[6];

    lockSemaphore(&shared_usart0_rx_buffer_lock);

    if (shared_usart0_rx_buffer_length == 6)
    {

        for (x = 0; x < 6; x++)
            reply[x] = shared_usart0_rx_buffer[x];

    }

    unlockSemaphore(&shared_usart0_rx_buffer_lock);

    if ( reply[0] == id && reply[1] == 4 && reply[2] == 0 && reply[5] == calculateCheckSum(reply[0], 0, &reply[3], 2) )
    {

        uint16_t position = ( (reply[4] << 8) | reply[3] ) / 4;

        if (position > 0xfe)
            position &= 0xfe;

        result =  (uint8_t) position;

    }

    setReplyStatus(REPLY_ON);

    return result;

} // getPosition

uint16_t getLoad(uint8_t id, uint8_t replyStatus)
{

    setReplyStatus(replyStatus);

    uint16_t result = 0;
    uint8_t x;
    uint8_t buffer[8];

    // 0xff 0xff id length instruction address length checksum
    buffer[0] = AX12_HEADER0;
    buffer[1] = AX12_HEADER1;
    buffer[2] = id;
    buffer[3] = 0x04;
    buffer[4] = AX12_READ_DATA;
    buffer[5] = AX12_PRESENT_LOAD_L;
    buffer[6] = 0x02;
    buffer[7] = calculateCheckSum(id, AX12_READ_DATA, &buffer[5], 0x02);

    lockSemaphore(&usart0_lock);
    transmitBufferUSART0(buffer, 8);
    unlockSemaphore(&usart0_lock);

    uint16_t start_time = ticks1;
    while( shared_usart0_rx_buffer_length < 6 || (ticks1 - start_time) < AX12_REPLY_TIMEOUT )
        asm volatile("nop"::);

    uint8_t reply[6];

    lockSemaphore(&shared_usart0_rx_buffer_lock);

    if (shared_usart0_rx_buffer_length == 6)
    {

        for (x = 0; x < 6; x++)
            reply[x] = shared_usart0_rx_buffer[x];

    }

    unlockSemaphore(&shared_usart0_rx_buffer_lock);

    if ( reply[0] == id && reply[1] == 4 && reply[2] == 0 && reply[5] == calculateCheckSum(reply[0], 0, &reply[3], 2) )
    {

        result = (reply[4] << 8) | reply[3];

    }

    setReplyStatus(REPLY_ON);

    return result;

} // getLoad

void motorOff(uint8_t id, uint8_t replyStatus)
{

    setReplyStatus(replyStatus);

    uint8_t buffer[8];

    // 0xff 0xff id length instruction address data1 checksum
    buffer[0] = AX12_HEADER0;
    buffer[1] = AX12_HEADER1;
    buffer[2] = id;
    buffer[3] = 0x04;
    buffer[4] = AX12_WRITE_DATA;
    buffer[5] = AX12_TORQUE_ENABLE;
    buffer[6] = 0x00;
    buffer[7] = calculateCheckSum(id, AX12_WRITE_DATA, &buffer[5], 0x02);

    lockSemaphore(&usart0_lock);
    transmitBufferUSART0(buffer, 8);
    unlockSemaphore(&usart0_lock);

    uint16_t start_time = ticks1;
    while( shared_usart0_rx_buffer_length < 4 || (ticks1 - start_time) < AX12_REPLY_TIMEOUT )
        asm volatile("nop"::);

    setReplyStatus(REPLY_ON);

} // motorOff

void motorOn(uint8_t id, uint8_t replyStatus)
{

    setReplyStatus(replyStatus);

    uint8_t buffer[8];

    // 0xff 0xff id length instruction address data1 checksum
    buffer[0] = AX12_HEADER0;
    buffer[1] = AX12_HEADER1;
    buffer[2] = id;
    buffer[3] = 0x04;
    buffer[4] = AX12_WRITE_DATA;
    buffer[5] = AX12_TORQUE_ENABLE;
    buffer[6] = 0x01;
    buffer[7] = calculateCheckSum(id, AX12_WRITE_DATA, &buffer[5], 0x02);

    lockSemaphore(&usart0_lock);
    transmitBufferUSART0(buffer, 8);
    unlockSemaphore(&usart0_lock);

    uint16_t start_time = ticks1;
    while( shared_usart0_rx_buffer_length < 4 || (ticks1 - start_time) < AX12_REPLY_TIMEOUT )
        asm volatile("nop"::);

    setReplyStatus(REPLY_ON);

} // motorOn

void ping(uint8_t id, uint8_t replyStatus)
{

    setReplyStatus(replyStatus);

    uint8_t buffer[6];

    // 0xff 0xff id length instruction checksum
    buffer[0] = AX12_HEADER0;
    buffer[1] = AX12_HEADER1;
    buffer[2] = id;
    buffer[3] = 0x02;
    buffer[4] = AX12_PING;
    buffer[5] = calculateCheckSum(id, AX12_PING, 0, 0x00);

    lockSemaphore(&usart0_lock);
    transmitBufferUSART0(buffer, 6);
    unlockSemaphore(&usart0_lock);

    uint16_t start_time = ticks1;
    while( shared_usart0_rx_buffer_length < 4 || (ticks1 - start_time) < AX12_REPLY_TIMEOUT )
        asm volatile("nop"::);

    setReplyStatus(REPLY_ON);

} // ping

void setPositionAndSpeed(uint8_t id, uint8_t position, uint8_t speed, uint8_t replyStatus)
{

    setReplyStatus(replyStatus);
    uint16_t p = position * 4;
    uint16_t s = speed * 4;
    uint8_t buffer[11];

    // 0xff 0xff id length instruction address data1 data2 data3 data4 checksum
    buffer[0] = AX12_HEADER0;
    buffer[1] = AX12_HEADER1;
    buffer[2] = id;
    buffer[3] = 0x07;
    buffer[4] = AX12_WRITE_DATA;
    buffer[5] = AX12_GOAL_POSITION_L;
    buffer[6] = (uint8_t) p;
    buffer[7] = (uint8_t) (p >> 8);
    buffer[8] = (uint8_t) s;
    buffer[9] = (uint8_t) (s >> 8);
    buffer[10] = calculateCheckSum(id, AX12_WRITE_DATA, &buffer[5], 0x05);

    lockSemaphore(&usart0_lock);
    transmitBufferUSART0(buffer, 11);
    unlockSemaphore(&usart0_lock);

    uint16_t start_time = ticks1;
    while( shared_usart0_rx_buffer_length < 4 || (ticks1 - start_time) < AX12_REPLY_TIMEOUT )
        asm volatile("nop"::);

    // temp
    if (id < 21)
        robot_state.position[id - 1] = position;
    setReplyStatus(REPLY_ON);

} // setPositionAndSpeed


void moveLoadCheck(uint8_t id, uint8_t direction, uint8_t replyStatus)
{

    setReplyStatus(replyStatus);

    readLoad(id);

    while( robot_state.load[id - 1] < 512 && robot_state.load[id - 1] > -512 )
    {

        switch(direction)
        {

        case DIRECTION_CW:
            setPositionAndSpeed(id, robot_state.position[id - 1] + 2, 128, REPLY_OFF);
            break;

        case DIRECTION_CCW:
            setPositionAndSpeed(id, robot_state.position[id - 1] - 2, 128, REPLY_OFF);
            break;

        default:
            break;

        }

        readLoad(id);

    }

    switch(direction)
    {

    case DIRECTION_CW:
        setPositionAndSpeed(id, robot_state.position[id - 1] - 10, 128, REPLY_OFF);
        break;

    case DIRECTION_CCW:
        setPositionAndSpeed(id, robot_state.position[id - 1] + 10, 128, REPLY_OFF);
        break;

    default:
        break;

    }

    if ( getReplyStatus() )
        protocolSuccess();

    setReplyStatus(REPLY_ON);

} // moveLoadCheck
*/

#endif //DISABLE_AX12
